﻿using MathNet.Numerics.LinearAlgebra;
using Prism.Commands;
using Prism.Mvvm;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Controls;
using System.Windows.Input;

namespace TestingPlatForm.MissionContrl.ViewModels
{
    public class RAHViewModel : BindableBase
    {

        #region
        private double _x_sa;

        public double x_sa
        {
            get { return _x_sa; }
            set { SetProperty(ref _x_sa, value); }
        }

        private double _y_sa;

        public double y_sa
        {
            get { return _y_sa; }
            set { SetProperty(ref _y_sa, value); }
        }

        private double _z_sa;

        public double z_sa
        {
            get { return _z_sa; }
            set { SetProperty(ref _z_sa, value); }
        }

        private double _vx_sa;

        public double vx_sa
        {
            get { return _vx_sa; }
            set { SetProperty(ref _vx_sa, value); }
        }

        private double _vy_sa;

        public double vy_sa
        {
            get { return _vy_sa; }
            set { SetProperty(ref _vy_sa, value); }
        }

        private double _vz_sa;

        public double vz_sa
        {
            get { return _vz_sa; }
            set { SetProperty(ref _vz_sa, value); }
        }

        private double _x_la;

        public double x_la
        {
            get { return _x_la; }
            set { SetProperty(ref _x_la, value); }
        }
        private double _y_la;

        public double y_la
        {
            get { return _y_la; }
            set { SetProperty(ref _y_la, value); }
        }

        private double _z_la;

        public double z_la
        {
            get { return _z_la; }
            set { SetProperty(ref _z_la, value); }
        }

        private double _Elevation;

        public double Elevation
        {
            get { return _Elevation; }
            set { SetProperty(ref _Elevation, value); }
        }

        private double _Azimuth;

        public double Azimuth
        {
            get { return _Azimuth; }
            set { SetProperty(ref _Azimuth, value); }
        }

#endregion

        public RAHViewModel()
        {
        }

        public ICommand CalCommand
        {
            get => new DelegateCommand(() => 
            {
                Vector<double> X = Vector<double>.Build.DenseOfArray(new double[] {x_la, y_la, z_la });
                Vector<double> RV = Vector<double>.Build.DenseOfArray(new double[] { x_sa, y_sa ,z_sa ,vx_sa ,vy_sa,vz_sa});
                Vector<double> Sat2SH_ECEF = Vector<double>.Build.DenseOfArray(new double[] { X[0] - RV[0], X[1] - RV[1], X[2] - RV[2] });
                Vector<double> Sat2SH_VVLH = ECEF2VVLH(1, RV, Sat2SH_ECEF);
                double temp = Math.Atan2(Sat2SH_VVLH[1], Sat2SH_VVLH[0]) * 180 / Math.PI;
                Elevation = Math.Round(Math.Acos(Sat2SH_VVLH[2] / CalculateEuclideanNorm(Sat2SH_VVLH)) * 180 / Math.PI, 4);
                // rah.R = rah.A - 90;
                 Azimuth = Math.Round((temp + 360) % 360,4);
            });
        }

        //public RAH Calcul(Vector<double> RV, Vector<double> X)
        //{
        //    Vector<double> Sat2SH_ECEF = Vector<double>.Build.DenseOfArray(new double[] { X[0] - RV[0], X[1] - RV[1], X[2] - RV[2] });
        //    Vector<double> Sat2SH_VVLH = ECEF2VVLH(1, RV, Sat2SH_ECEF);
        //    RAH rah = new RAH();
        //    double temp = Math.Atan2(Sat2SH_VVLH[1], Sat2SH_VVLH[0]) * 180 / Math.PI;
        //    rah.A = Math.Acos(Sat2SH_VVLH[2] / CalculateEuclideanNorm(Sat2SH_VVLH)) * 180 / Math.PI;
        //    rah.R = rah.A - 90;
        //    rah.H = (temp + 360) % 360;
        //    return rah;
        //}

        public static Vector<double> ECEF2VVLH(int Model, Vector<double> RV, Vector<double> X)
        {
            double We = 0.000072921150;
            Vector<double> RVTemp = Vector<double>.Build.DenseOfArray(RV.ToArray());

            // 速度修正
            if (Model > 0)
            {
                RVTemp[3] = RV[3] - RV[1] * We;
                RVTemp[4] = RV[4] + RV[0] * We;
            }

            // R 和 V
            Vector<double> R = RVTemp.SubVector(0, 3);
            Vector<double> V = RVTemp.SubVector(3, 3);

            // 计算所需向量和正规化
            double r = R.L2Norm();
            Vector<double> H = CrossProduct(R, V);
            double h = H.L2Norm();

            // 构建旋转矩阵
            Matrix<double> Roi = Matrix<double>.Build.Dense(3, 3);
            Roi.SetRow(2, (-R / r).ToArray());
            Roi.SetRow(1, (-H / h).ToArray());
            Roi.SetRow(0, CrossProduct(Roi.Row(1), Roi.Row(2)));

            // 应用模型转换
            if (Math.Abs(Model) == 1)
            {
                return Roi * X;
            }
            else if (Math.Abs(Model) == 2)
            {
                return Roi.Transpose() * X;
            }

            return null; // 如果模型不是 1 或者 -1, 2 或者 -2 返回null
        }

        public static Vector<double> CrossProduct(Vector<double> vector1, Vector<double> vector2)
        {
            // 叉积的计算公式：(v1y*v2z - v1z*v2y, v1z*v2x - v1x*v2z, v1x*v2y - v1y*v2x)
            Vector<double> result = Vector<double>.Build.DenseOfArray(new double[] { 0, 0, 0 }); ;
            result[0] = vector1[1] * vector2[2] - vector1[2] * vector2[1];
            result[1] = vector1[2] * vector2[0] - vector1[0] * vector2[2];
            result[2] = vector1[0] * vector2[1] - vector1[1] * vector2[0];
            return result;
        }

        public static double CalculateEuclideanNorm(Vector<double> vector)
        {
            double sumOfSquares = 0;

            foreach (double element in vector)
            {
                sumOfSquares += element * element;
            }

            return Math.Sqrt(sumOfSquares);
        }
    }
}
